/*
    FILE: dbscan.h
    ------------------
    helper class function definitions for dbscan
*/
#include <onboard_detector/dbscan.h>
#include <iostream>

namespace onboardDetector{
    int DBSCAN::run()
    {
        int clusterID = 1;
        vector<Point>::iterator iter;
        for(iter = m_points.begin(); iter != m_points.end(); ++iter)
        {
            if ( iter->clusterID == UNCLASSIFIED )
            {
                if ( expandCluster(*iter, clusterID) != FAILURE )
                {
                    clusterID += 1;
                }
            }
        }

        return 0;
    }

    int DBSCAN::expandCluster(Point point, int clusterID)
    {    
        vector<int> clusterSeeds = calculateCluster(point);

        if ( clusterSeeds.size() < m_minPoints )
        {
            point.clusterID = NOISE;
            return FAILURE;
        }
        else
        {
            int index = 0, indexCorePoint = 0;
            vector<int>::iterator iterSeeds;
            for( iterSeeds = clusterSeeds.begin(); iterSeeds != clusterSeeds.end(); ++iterSeeds)
            {
                m_points.at(*iterSeeds).clusterID = clusterID;
                if (m_points.at(*iterSeeds).x == point.x && m_points.at(*iterSeeds).y == point.y && m_points.at(*iterSeeds).z == point.z )
                {
                    indexCorePoint = index;
                }
                ++index;
            }
            clusterSeeds.erase(clusterSeeds.begin()+indexCorePoint);

            for( vector<int>::size_type i = 0, n = clusterSeeds.size(); i < n; ++i )
            {
                vector<int> clusterNeighors = calculateCluster(m_points.at(clusterSeeds[i]));

                if ( clusterNeighors.size() >= m_minPoints )
                {
                    vector<int>::iterator iterNeighors;
                    for ( iterNeighors = clusterNeighors.begin(); iterNeighors != clusterNeighors.end(); ++iterNeighors )
                    {
                        if ( m_points.at(*iterNeighors).clusterID == UNCLASSIFIED || m_points.at(*iterNeighors).clusterID == NOISE )
                        {
                            if ( m_points.at(*iterNeighors).clusterID == UNCLASSIFIED )
                            {
                                clusterSeeds.push_back(*iterNeighors);
                                n = clusterSeeds.size();
                            }
                            m_points.at(*iterNeighors).clusterID = clusterID;
                        }
                    }
                }
            }

            return SUCCESS;
        }
    }

    vector<int> DBSCAN::calculateCluster(Point point)
    {
        int index = 0;
        vector<Point>::iterator iter;
        vector<int> clusterIndex;
        for( iter = m_points.begin(); iter != m_points.end(); ++iter)
        {
            if ( calculateDistance(point, *iter) <= m_epsilon )
            {
                clusterIndex.push_back(index);
            }
            index++;
        }
        return clusterIndex;
    }

    inline double DBSCAN::calculateDistance(const Point& pointCore, const Point& pointTarget )
    {
        return pow(pointCore.x - pointTarget.x,2)+pow(pointCore.y - pointTarget.y,2)+pow(pointCore.z - pointTarget.z,2);
    }
}


